/*************************************************************
*
* M3D_LERP.CPP
*
* More involved interpolations
*************************************************************/
#include <m3d/m3d_stdafx.h>

#include <ap_comm.h>
#include <m3d.h>

/*************************************************************
* m3dClampVector_L()
*
* IN:  vSrc     - source vector (first interpolant)
*      vDest    - destination vector (second interpolant)
*      maxLen   - maximal length of movement from vSrc to vDest
* OUT: vOut     - vector in between vSrc and vLen
*                 (per-coordinate interpolation is used)
* RET: TRUE     - interpolation completed (vOut = vDest)
*      FALSE    - interpolation between vSrc and vDest 
*                 is not completed
*
* Linear interpolation with clamping between two vectors
*************************************************************/
BOOL m3dClampVector_L (m3dV *vSrc, m3dV *vDest, float maxLen, m3dV *vOut)
{
   m3dV  dir;
   float l, len;

   m3dSubtractVector(vDest, vSrc, &dir);
   len = m3dLengthVector(&dir);
   if (m3dIsZero_A(len, 100*M3D_EPSILON)) {
      *vOut = *vSrc;
      return TRUE;
   }
   l = _m3dClamp(0.f, maxLen, len);
   _m3dLineCombine(vSrc, &dir, l/len, vOut);

   return (l < len - 100.f*M3D_EPSILON) ? FALSE:TRUE;
}

/*************************************************************
* m3dClampVector_L()
*
* Linear interpolation with clamping between two vectors
* The same parameters as in m3dClampVector_L() above, just
* the result is stored directly in vDest
*************************************************************/
BOOL m3dClampVector_L (m3dV *vSrc, m3dV *vDest, float maxLen)
{
   m3dV  v;
   BOOL  rc;

   rc = m3dClampVector_L(vSrc, vDest, maxLen, &v);
   *vDest = v;
   return rc;
}

/*************************************************************
* m3dClampVector_A()
*
* IN:  vSrc     - source vector (first interpolant)
*      vDest    - destination vector (second interpolant)
*      stepAngle - maximal angle of turn between two vectors
* OUT: vOut     - vector, lying in the plane(vSrc, vDest), 
*                 between vSrc and vDest, 
*                 angle(vSrc,vOut) <= maxAngle
*
* Angular (spherical) interpolation in XZ with clamping 
* between two vectors
*************************************************************/
BOOL  m3xzClampVector_A (m3dV *vSrc, const m3dV *vDest, float stepAngle, m3dV *vOut)
{
   m3dV _vSrc  = *vSrc;
   m3dV _vDest = *vDest;
   m3dNormalize(&_vSrc);         
   m3dNormalize(&_vDest);

   // angle
   float ang, angle = m3xzAngleVector(&_vSrc,&_vDest);

   if (m3dFAbs(angle) < 0.1f) {
      *vOut  = *vDest;
      return FALSE;
   }

   // calculate the actual angle we'll turn vSrc towards vDest
   if (angle > 0.f) {
      ang = _m3dClamp(0.f, stepAngle, angle);
   } else {
      ang = _m3dClamp(0.f, -stepAngle, angle);
   }

   m3dMATR matRot;
   matRot.MakeRotY(ang);
   matRot.TransformVector(&_vSrc, vOut);

   // interpolate length
   float len = m3dLerp(m3dLengthVector(vSrc),m3dLengthVector(vDest),0.f, m3dFAbs(angle), m3dFAbs(ang));
   _m3dScaleVector(vOut, len);

   // TRUE => finished interpolating
   return (ang < angle - 100.f*M3D_EPSILON) ? FALSE:TRUE;
}

/*************************************************************
* m3dClampVector_A()
*
* IN:  vSrc     - source vector (first interpolant)
*      vDest    - destination vector (second interpolant)
*      maxAngle - maximal angle of turn between two vectors
*      vPolar   - not used so far
*      options  - not used so far
* OUT: vOut     - vector, lying in the plane(vSrc, vDest), 
*                 between vSrc and vDest, 
*                 angle(vSrc,vOut) <= maxAngle
* RET: TRUE     - interpolation completed (vOut = vDest)
*      FALSE    - interpolation between vSrc and vDest 
*                 is not completed
*
* Angular (spherical) interpolation with clamping 
* between two vectors
*************************************************************/
BOOL m3dClampVector_A (m3dV *vSrc, const m3dV *vDest, float maxAngle, m3dV *vOut)
{
   // check for valid clamp cone
   ASSERT(maxAngle > -M3D_EPSILON);
   
   m3dV     _vSrc  = *vSrc;
   m3dV     _vDest = *vDest;
   m3dNormalize(&_vSrc);
   m3dNormalize(&_vDest);

   m3dV     vPerp;
   m3dCrossProduct(&_vSrc,&_vDest,&vPerp);
   float angle = m3dAngleVector(&_vSrc,&_vDest);
   ASSERT((angle > -M3D_EPSILON) && (angle < (180.f + 100*M3D_EPSILON)));

   if (angle < 0.1f) {
      // vSrc and vDest are nearly collinear
      *vOut = _vDest;
      return TRUE;
   }

   // calculate the actual angle we'll turn vSrc towards vDest
   float ang = _m3dClamp(0.f,maxAngle,angle);

   if (m3dIsZero_A(m3dLengthVector(&vPerp),100*M3D_EPSILON)) {
      // need patch this case to avoid normalize(zero);
      m3dV vArbitrary;
      if (m3dFAbs(m3dDotProduct(&_vSrc, &m3dVUnitY)) > 0.5f) {
         vArbitrary = m3dVUnitX;
      } else {
         vArbitrary = m3dVUnitY;
      }
      m3dTangentComponent(&vArbitrary, &_vSrc, &vPerp);
   }
   m3dNormalize(&vPerp);

   // calculate the third vector of LCS
   m3dV     vThird;
   m3dCrossProduct(&_vSrc,&vPerp,&vThird);
   m3dMATR  mat, matRot;
   mat.MakeWCS2LCS(&m3dVZero,&_vSrc,&vPerp,&vThird);
   // in LCS vSrc becomes vX; vPerp becomes vY, vThird becomes vZ
   matRot.MakeRotY(ang);

   // v - vector in the plane (vSrc,vDest), angle(vSrc,v) = ang
   m3dV v;
   matRot.TransformVector(&m3dVUnitX,&v);
   
   // matInv: (vSrc,vPerp,vThird) -> WCS)
   m3dMATR  matInv;
   if (!mat.Invert(&matInv)) {
      ASSERT(FALSE);
   }

   matInv.TransformVector(&v,vOut);

   // interpolate length
   float len = m3dLerp(m3dLengthVector(&_vSrc),m3dLengthVector(&_vDest),0.f,angle,ang);
   _m3dScaleVector(vOut,len);
   {
      float a;
      a = m3dAngleVector(&_vSrc,vOut);
      ASSERT(m3dIsZero_A(a-ang,0.1f));
      ASSERT(m3dIsZero_A(m3dDotProduct(vOut,&vPerp),0.01f));
   }

   // TRUE => finished interpolating
   return (ang < angle - 100.f*M3D_EPSILON) ? FALSE:TRUE;
}

/*************************************************************
* m3dLerpVector_L ()
*
* Lerp vector linearly (each coordinate is lerped).
* Lerping is parametrized by parameter 't'.
*
* IN:  vA    - vector, corresponding to t = a
*      vB    - vector, corresponding to t = b
*      a, b  - specify the interval (domain) of parameter 't'
*      t     - parameter of linear interpolation
* OUT: vOut  - result of interpolation
*************************************************************/
void m3dLerpVector_L (m3dV *vA, m3dV *vB, float a, float b, float t, m3dV *vOut)
{
   vOut->x = m3dLerp(vA->x,vB->x,a,b,t);
   vOut->y = m3dLerp(vA->y,vB->y,a,b,t);
   vOut->z = m3dLerp(vA->z,vB->z,a,b,t);
   return;
}

/*************************************************************
* m3dLerpVector_A ()
*
* Lerp vector angularly (through rotation, length is not lerped ).
* Lerping is parametrized by parameter 't'.
*
* IN:  vA    - vector, corresponding to t = a
*      vB    - vector, corresponding to t = b
*      a, b  - specify the interval (domain) of parameter 't'
*      t     - parameter of linear interpolation
* OUT: vOut  - result of interpolation
*************************************************************/
void m3dLerpVector_A (m3dV *vA, m3dV *vB, float a, float b, float t, m3dV *vOut)
{
   float fullAngle = m3dAngleVector(vA, vB);
   float angle     = m3dClamp(0.f, fullAngle, a, b, t);

   m3dQUAT quat;
   quat.SetClampRot(vA, vB, angle);
   quat.Transform(vA, vOut);

   return;
}

/*************************************************************
* m3dClampVector ()
*
* Clamp vector linearly (each coordinate is lerped).
* Lerping is parametrized by parameter 't'.
*
* IN:  vA    - vector, corresponding to t = a
*      vB    - vector, corresponding to t = b
*      a, b  - specify the interval (domain) of parameter 't'
*      t     - parameter of linear interpolation
* OUT: vOut  - result of interpolation
*************************************************************/
void m3dClampVector (const m3dV *vA, const m3dV *vB, float a, float b, float t, m3dV *vOut)
{
   vOut->x = m3dClampZero(vA->x,vB->x,a,b,t);
   vOut->y = m3dClampZero(vA->y,vB->y,a,b,t);
   vOut->z = m3dClampZero(vA->z,vB->z,a,b,t);
   return;
}

/***************************************************************************
* m3dLerpRectCorners()
*
* Pseudo-linear interpolation of the function by its values 
* in the corners of rectangle
*                               ________________pFur
*                              |                |
* IN:  pBll, pFur - rectangle  |.....pFrom      |
*                              |     .          |
*                              |     .          |
*                              |     .          |
*                              |     .          |
*                              |_____.__________|
*                            pBll
*
*      fBllBll - the function value in the left-lower  corner (pBll)
*      fBllFur - -----------||------------ left-upper  corner
*      fFurBll - -----------||------------ right-lower corner
*      fFurFur - -----------||------------ right-upper corner (pFur)
* OUT: none
* RET: the interpolated value of the function in the point pFrom
***************************************************************************/
float m3dLerpRectCorners(m2dV *pBll, m2dV *pFur, m2dV *pFrom,
                         float fBllBll, float fBllFur, float fFurBll, float fFurFur)
{
   float f_low, f_up, f_right, f_left;
   float f_vert_hor, f_hor_vert;

   ASSERT(pBll->x <= pFur->x);
   ASSERT(pBll->y <= pFur->y);

   f_low      = m3dLerpZero(fBllBll, fFurBll, pBll->x, pFur->x, pFrom->x);
   f_up       = m3dLerpZero(fBllFur, fFurFur, pBll->x, pFur->x, pFrom->x);
   f_hor_vert = m3dLerpZero(f_low, f_up, pBll->y, pFur->y, pFrom->y);

   f_left     = m3dLerpZero(fBllBll, fBllFur, pBll->y, pFur->y, pFrom->y);
   f_right    = m3dLerpZero(fFurBll, fFurFur, pBll->y, pFur->y, pFrom->y);
   f_vert_hor = m3dLerpZero(f_left, f_right, pBll->x, pFur->x, pFrom->x);
   
   return 0.5f * (f_vert_hor + f_hor_vert);
}

//
// End-of-file 'M3D_LERP.CPP'
//

